#include <stdio.h>
#include <inc/maylloc.h>
#include <math.h>
#include <string.h>

#include "inc/page.h"

static __thread struct free_entry free_list[FREE_LIST_SIZE];

char ff_0FF[BITMAP_SIZE]	= {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

static size_t free_list_index(size_t size) {
  size_t round    = size*2-1;
  size_t highest  = 0;
  while((round=round>>1))
    highest++;
  return LISTINDEX(highest);//(size_t)(log((double)(size*2-1))/log(2.0)));
}


static int find_free_block(struct page_header *page_hdr) {
  int blockno=0;
  int index;
  int diff = 0;
  char *bitmap = page_hdr->bitmap;
  char bits;
  
	asm ("cld\n\t"
		"repe cmpsb\n\t"
		: "=c" (diff)
		: "c" (BITMAP_SIZE), "S" (bitmap), "D" (ff_0FF));
	index = BITMAP_SIZE - diff - 1;
	blockno = index * 8;
	bits = bitmap[BITMAP_SIZE - diff - 1];
      while( bits & (char)1 ){
        blockno++;
        bits = bits >> 1;
      }

	if(blockno < NUMBLOCKS(INDEX2SIZE(page_hdr->size))){
		SET_BITMAP(page_hdr->bitmap, blockno);
		return blockno;
	}
	else
		return -1;
}

static void zero_list(void) {
	memset (free_list, 0, sizeof(free_list));
}

void *malloc(size_t size) {
  struct free_entry *entry  = NULL;
  size_t index  = free_list_index(size);
  int blockno;
  void *ret_addr = NULL;

  if (size <= (PAGE_SIZE/2)) {
    entry = &free_list[index];
  } else {
    size_t num_pages  = NUM_PAGES_FROM_SIZE(size+sizeof(struct large_page_header));
    struct large_page_header *new = allocate_mmap_pages(num_pages);
    if (new) {
      init_large_page(new, num_pages);
      return (void *)((int)new+sizeof(struct large_page_header));
    }
    return NULL;
  }
  if (entry->free_start != NULL) {
    //If Full Do Something.
    if((blockno = find_free_block(entry->free_start)) >= 0)
		  ret_addr = BLOCKNO2PTR(entry->free_start, blockno, INDEX2SIZE(entry->free_start->size));
  }
  else {
    struct page_header *new = allocate_new_page(PAGE_SIZE);
    if (new != NULL) {
      init_page(new, index);
      // Add to free list.
      entry->free_start = new;
      entry->free_end   = new;
	  if((blockno = find_free_block(new)) >= 0)
		  ret_addr = BLOCKNO2PTR(new, blockno, INDEX2SIZE(new->size));
    }
  }

  if (entry->free_start && ret_addr) {
    entry->free_start->allocated_blocks++;
    if (entry->free_start->allocated_blocks == NUMBLOCKS(INDEX2SIZE(entry->free_start->size))) {
      struct page_header *ptr = entry->free_start;
      entry->free_start = ptr->free_ptr;
      if (entry->free_start == NULL)
        entry->free_end = NULL;
      ptr->free_ptr = NULL;
    }
  }
  return ret_addr;
}

void free(void *ptr) {
  struct page_header *page_base  = ROUNDDOWN(ptr);
  size_t size = 0;
  if (!page_base)
    return;

  size  = page_base->size;
  if ((size&LARGE_PAGE_MARKER) == 0) {
    int blockno = BLOCKNO(ptr, INDEX2SIZE(size));
	  RESET_BITMAP(page_base->bitmap, blockno);
    if (page_base->allocated_blocks == NUMBLOCKS(INDEX2SIZE(size))) {
      if (free_list[size].free_end == NULL) {
        free_list[size].free_end    = page_base;
        free_list[size].free_start  = page_base;
      }
      else {
        struct page_header *swap = free_list[size].free_end;
        page_base->free_ptr = NULL;
        free_list[size].free_end = page_base;
        swap->free_ptr  = page_base;
      }
    }
    page_base->allocated_blocks--;
  } else
    free_mmap_pages(page_base, REMOVE_LARGE_PAGE_MARKER(size));
}

void *realloc(void * ptr, size_t size) {
  void *ret_ptr = NULL;
  struct page_header *base  = ROUNDDOWN(ptr);
  if (!base) {
    void *a = malloc(size);
    return a;
  }

  if ((base->size&LARGE_PAGE_MARKER) == 0) {
    if (LISTINDEX(free_list_index(size)) <= base->size)
      return ptr;
    else {
      if (!(ret_ptr=malloc(size))) {
        return NULL;
      }
      else {
        memcpy(ret_ptr, ptr, INDEX2SIZE(base->size));
        free(ptr);
        return ret_ptr;
      }
    }
  } else {
    if (NUM_PAGES_FROM_SIZE(size)<=REMOVE_LARGE_PAGE_MARKER(base->size))
      return ptr;
    else {
      return allocate_remap_pages(ptr, NUM_PAGES_FROM_SIZE(size));
    }
  }
}

void *calloc(size_t num, size_t size) {
  return malloc(num*size);
}
